/*
 * ramp.h
 *
 *  Created on: 2024年11月4日
 *      Author: zizhong
 */

#ifndef CONTROLLIB_RAMP_H_
#define CONTROLLIB_RAMP_H_

#include "stdint.h"

typedef struct {
    float TargetValue;
    // Input: Target input (pu)

    uint32_t  RampDelayMax;
    // Parameter: Maximum delay rate (Q0) - independently with global Q

    float RampLowLimit;
    // Parameter: Minimum limit (pu)

    float RampHighLimit;
    // Parameter: Maximum limit (pu)

    uint32_t  RampDelayCount;
    // Variable: Incremental delay (Q0) - independently with global Q

    float SetpointValue;
    // Output: Target output (pu)

    uint32_t  EqualFlag;
    // Output: Flag output (Q0) - independently with global Q

    float Tmp;
    // Variable: Temp variable
} RAMP_ctl_t;

typedef struct
{ float  Freq;
// Input: Ramp frequency (pu)
  float  StepAngleMax;
  // Parameter: Maximum step angle (pu)
  float  Angle;
  // Variable: Step angle (pu)
  float  Gain;
  // Input: Ramp gain (pu)
  float  Out;
  // Output: Ramp signal (pu)
  float  Offset;
  // Input: Ramp offset (pu)
} RAMP_gen_t;

#define RAMPGEN_DEFAULTS { \
        0, /* Freq */ \
        0, /* StepAngleMax */ \
        0, /* Angle */ \
        1, /* Gain */ \
        0, /* Out */ \
        1  /* Offset */ \
    }

static inline void rampGenerateStep(RAMP_gen_t * in)
{
    // Compute the angle rate
    in->Angle += in->StepAngleMax * in->Freq;

    // Saturate the angle rate within (-1,1)
    if(in->Angle > 1.0f)
    {
        in->Angle -= 1.0f;
    }
    else if(in->Angle < -1.0f)
    {
        in->Angle += 1.0f;
    }

    in->Out = in->Angle;
}

//这个函数通过逐步调整设定值来接近目标值，同时确保设定值在指定的范围内
static inline void rampControlStep(RAMP_ctl_t * rampController)
{
    rampController->Tmp = (rampController->TargetValue) - (rampController->SetpointValue);

    if(fabsf(rampController->Tmp) >= 0.00001525f)
    {
        rampController->RampDelayCount++;

        if((rampController->RampDelayCount) >= (rampController->RampDelayMax))
        {
            if(rampController->TargetValue >= rampController->SetpointValue)
            {
                rampController->SetpointValue += 0.00001525f;
            }
            else
            {
                rampController->SetpointValue -= 0.00001525f;
            }

            rampController->RampDelayCount = 0;
        }
    }
    else
    {
        rampController->SetpointValue = rampController->TargetValue;
        rampController->EqualFlag = 0x7FFFFFFF;
    }

    rampController->SetpointValue = __fmax((__fmin(rampController->SetpointValue, rampController->RampHighLimit)),rampController->RampLowLimit);

    rampController->SetpointValue  = (rampController->SetpointValue  > rampController->RampHighLimit)
                                     ? rampController->RampHighLimit : rampController->SetpointValue;
    rampController->SetpointValue  = (rampController->SetpointValue  < rampController->RampLowLimit)
                                     ? rampController->RampLowLimit : rampController->SetpointValue;
}

static inline float ramper(float in, float out, float rampDelta)
{
    float err;

    err = in - out;

    if(err > rampDelta)
    {
        return(out + rampDelta);
    }
    else if(err < -rampDelta)
    {
        return(out - rampDelta);
    }
    else
    {
        return(in);
    }
}





#endif /* CONTROLLIB_RAMP_H_ */
